#include "driverDSMX_UART_ISR.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
#include "timers.h"

#include <p32xxxx.h>
#include "main_peripherals.h"
#include "state.h"

TimerHandle_t dsmxTimer;

struct
{
	BYTE rcReadBuffer[16];
	int rcReadBufferPos;
    BYTE misAligned;
} _rc;

TickType_t rcCheck[2];
int rcCheckPos = 0;

BYTE rcCountValidChannels()
{
    int i;
    BYTE channel;
    BYTE count = 0;
	for (i=1;i<7;i++)
	{
		channel = (_rc.rcReadBuffer[i*2] & 0x78) >> 3;
		if (channel > 7) continue;

        count++;
	}

    return count >= 3 && count <= 5 ? 1 : 0;
}

void rcParse()
{
    BYTE i;
	BYTE channel;
	SHORT value;

    rcCheck[1] = xTaskGetTickCountFromISR();

    // misaligned!
    if (rcCheck[1] - rcCheck[0] > configTICK_RATE_HZ / 500)
    {
        rcCheck[0] = rcCheck[1];
        _rc.misAligned = 1;

        U4STACLR = _U4STA_URXISEL0_MASK | _U4STA_URXISEL1_MASK;
        //UARTSetFifoMode			(UART_RC, UART_INTERRUPT_ON_TX_DONE | UART_INTERRUPT_ON_RX_NOT_EMPTY);
        return;
    }
    /*if (_rc.rcReadBuffer[15] != 0xFF || !rcCountValidChannels())
    {
        // switch back to single byte ISR
        UARTSetFifoMode			(UART_RC, UART_INTERRUPT_ON_TX_DONE | UART_INTERRUPT_ON_RX_NOT_EMPTY);
        _rc.rcReadBufferPos = -1;
        _rc.misAlign++;
        return;
    }*/
    // switch to ISR on RX full => 4 bytes per ISR
    //UARTSetFifoMode			(UART_RC, UART_INTERRUPT_ON_TX_DONE | UART_INTERRUPT_ON_RX_FULL);

    LEDB_TOGGLE;
    
	//STATE.quadState.rfConnected = 1;
	STATE.rc.lastReceived = xTaskGetTickCountFromISR();

	for (i=1;i<7;i++)
	{
		channel = (_rc.rcReadBuffer[i*2] & 0x78) >> 3;
		if (channel >= 8) continue;

		value = ((_rc.rcReadBuffer[i*2] & 0x07) << 8) | _rc.rcReadBuffer[i*2+1];
        value -= 1024;

        STATE.rc.oldAxis[channel] = STATE.rc.axis[channel];
        STATE.rc.axis[channel] = value;
        STATE.rc.axisQ[channel] = value << 6;
        STATE.rc.channelChanged |= (1 << channel);
    }
    logStore(xTaskGetTickCountFromISR(), LOG_ID_RC_INPUT, STATE.rc.axis);

    //UARTSetFifoMode			(UART_RC, UART_INTERRUPT_ON_TX_DONE | UART_INTERRUPT_ON_RX_NOT_EMPTY);
}

void __attribute__((vector(_UART_4_VECTOR), interrupt(ipl5))) ISRWRAPPER_RC_UART();

void ISR_RC_UART()
{
	volatile char c;

    if (IFS2bits.U1BEIF)
    //if (INTGetFlag(INT_SOURCE_UART_ERROR(UART_RC)))
    {
        if (U4STAbits.OERR) U4STACLR = _U4STA_OERR_MASK;
        IFS2CLR = _IFS2_U1BEIF_MASK;
        //INTClearFlag(INT_SOURCE_UART_ERROR(UART_RC));
    }

    if (IFS2bits.U1BRXIF)
	//if (INTGetFlag(INT_SOURCE_UART_RX(UART_RC)))
	{
		while (U4STAbits.URXDA) // UARTReceivedDataIsAvailable(UART_RC))
		{
			c = U4RXREG; //UARTGetDataByte(UART_RC);

            if (_rc.misAligned)
            {
                rcCheck[1] = xTaskGetTickCountFromISR();
                if (rcCheck[1] - rcCheck[0] > configTICK_RATE_HZ / 500)
                {
                    _rc.misAligned = 0;
                    rcCheck[0] = rcCheck[1];
                    _rc.rcReadBufferPos = 0;
                    _rc.rcReadBuffer[_rc.rcReadBufferPos] = c;
                    _rc.rcReadBufferPos++;
                }
                else
                {
                    rcCheck[0] = rcCheck[1];
                }
            }
            else
            {
                if (_rc.rcReadBufferPos == 0) rcCheck[0] = xTaskGetTickCountFromISR();
                _rc.rcReadBuffer[_rc.rcReadBufferPos] = c;

                _rc.rcReadBufferPos++;

                if (_rc.rcReadBufferPos == 16)
                {
                    _rc.rcReadBufferPos = 0;
                    rcParse();
                }
            }
		}

        IFS2CLR = _IFS2_U1BRXIF_MASK;
        //INTClearFlag(INT_SOURCE_UART_RX(UART_RC));
	}

	portEND_SWITCHING_ISR(0);
}
